/* --------------------------------------------------------------------------
 * SimpleOpenNI User3d Test
 * --------------------------------------------------------------------------
 * Processing Wrapper for the OpenNI/Kinect library
 * http://code.google.com/p/simple-openni
 * --------------------------------------------------------------------------
 * prog:  Max Rheiner / Interaction Design / zhdk / http://iad.zhdk.ch/
 * date:  02/16/2011 (m/d/y)
 * ----------------------------------------------------------------------------
 * this demos is at the moment only for 1 user, will be implemented later
 * ----------------------------------------------------------------------------
 */
 
import SimpleOpenNI.*;


SimpleOpenNI context;
float        zoomF =0.5f;
float        rotX = radians(180);  // by default rotate the hole scene 180deg around the x-axis, 
                                   // the data from openni comes upside down
float        rotY = radians(0);

int timer = 0;
int lastFrameMillis = 0;
int currentFrameMillis = 0;

int throwTimer = 0;

float currentFrameElbowHandDifference;
float lastFrameElbowHandDifference;
float handAcceleration;

float directionFactor;

ArrayList elbowHandDifferences;
ArrayList frameMillis;

PImage splatterImage;


void setup()
{
  splatterImage = loadImage("blob.png");
  size(1024,768,P3D);  // strange, get drawing error in the cameraFrustum if i use P3D, in opengl there is no problem
  context = new SimpleOpenNI(this);
  
  elbowHandDifferences = new ArrayList();
  frameMillis = new ArrayList();
  // disable mirror
  context.setMirror(false);

  // enable depthMap generation 
  context.enableDepth();

  // enable skeleton generation for all joints
  context.enableUser(SimpleOpenNI.SKEL_PROFILE_ALL);

  stroke(255,255,255);
  smooth();  
  perspective(radians(45),
              float(width)/float(height),
              10,150000);
 }

void draw()
{
  currentFrameMillis = millis();
  frameMillis.add(currentFrameMillis);
  if(frameMillis.size() > 3) {
    frameMillis.remove(0); 
  }

  // update the cam
  context.update();

  //background(255,255,255);
  
  // set the scene pos
  translate(width/2, height/2, 0);
  rotateX(rotX);
  rotateY(rotY);
  scale(zoomF);
  
  //drawSkeleton(1);
  
  int[]   depthMap = context.depthMap();
  int     steps   = 3;  // to speed up the drawing, draw every third point
  int     index;
  PVector realWorldPoint;
 
  translate(0,0,-1000);  // set the rotation center of the scene 1000 infront of the camera

  stroke(100); 
  /*for(int y=0;y < context.depthHeight();y+=steps)
  {
    for(int x=0;x < context.depthWidth();x+=steps)
    {
      index = x + y * context.depthWidth();
      if(depthMap[index] > 0)
      { 
        // draw the projected point
        realWorldPoint = context.depthMapRealWorld()[index];
        point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);
      }
    } 
  } */
  PVector rightHandPos= new PVector();
  float confidence = context.getJointPositionSkeleton(1,SimpleOpenNI.SKEL_LEFT_HAND,rightHandPos);
  //System.out.println("X: " + rightHandPos.x + " Y: " + rightHandPos.y + " Z: " + rightHandPos.z);
  // draw the skeleton if it's available
  
  PVector rightElbowPos = new PVector();
  confidence = context.getJointPositionSkeleton(1,SimpleOpenNI.SKEL_RIGHT_ELBOW,rightElbowPos);
  PVector leftElbowPos = new PVector();
  confidence = context.getJointPositionSkeleton(1,SimpleOpenNI.SKEL_LEFT_ELBOW,leftElbowPos);

  PVector rightShoulderPos = new PVector();
  confidence = context.getJointPositionSkeleton(1,SimpleOpenNI.SKEL_RIGHT_SHOULDER,rightShoulderPos);


  
  fill(128,0,0);
  ellipse(-170,125,10,10);
  
  if(throwTimer > 0) {
    throwTimer--; 
  }
  
  if(context.isTrackingSkeleton(1)) {
    //handAcceleration = elbowHandDifference - rightHandPos.z - rightElbowPos.z;
    currentFrameElbowHandDifference = rightHandPos.z - rightElbowPos.z;
    elbowHandDifferences.add(new Float(currentFrameElbowHandDifference));
    if(elbowHandDifferences.size() > 3) {
      elbowHandDifferences.remove(0); 
    }
    //println(1000*((currentFrameElbowHandDifference - lastFrameElbowHandDifference) / ((currentFrameMillis - lastFrameMillis)*(currentFrameMillis - lastFrameMillis))));
    if((throwTimer == 0) &&
    ((acceleration() < -100))) {
      throwTimer = 25;
      if(rightHandPos.x > rightShoulderPos.x) {
        directionFactor = -4;
      } else {
        directionFactor = 4; 
      }
      translate(-rightHandPos.x*directionFactor, rightHandPos.y,rightHandPos.z);
      fill(rightHandPos.z%255,rightHandPos.z%255,rightHandPos.z%255);
      box(100);
      //image(splatterImage, 0, 0, 200, 200);
    } else {
      //fill(255,255,255);
      //ellipse(170,125,10,10);      
    }
    fill(0,128,0);
    ellipse(-170,125,10,10);//drawSkeleton(1);
    //println("x: " + rightHandPos.x);//x sivuttaissuunta 
    //println("y: " + rightHandPos.y);//y pystysuunta
    //println("z: " + rightHandPos.z);//z syvyys
    //if((abs(rightElbowPos.y - rightHandPos.y) < 60) || (abs(leftElbowPos.y - rightHandPos.y) < 60)) {
      //translate(-rightHandPos.x - abs(rightElbowPos.y - rightHandPos.y)/8, rightHandPos.y-100, rightHandPos.z); 
    if(rightHandPos.z < 1000) {
      translate(-rightHandPos.x,rightHandPos.y,rightHandPos.z);
      fill(rightHandPos.z%255,rightHandPos.z%255,rightHandPos.z%255);
      //box(100);
      image(splatterImage, 0, 0, 200, 200);
    }
  }
  // draw the kinect cam
  //context.drawCamFrustum();

  lastFrameMillis = currentFrameMillis;
  lastFrameElbowHandDifference = currentFrameElbowHandDifference;
}

float acceleration() {
  float acc = 0;
  if(elbowHandDifferences.size() < 4) {
    return 1000*((currentFrameElbowHandDifference - lastFrameElbowHandDifference) / ((currentFrameMillis - lastFrameMillis)*(currentFrameMillis - lastFrameMillis)));
  //   return 0;
  }
  //((1000*((currentFrameElbowHandDifference - lastFrameElbowHandDifference) / ((currentFrameMillis - lastFrameMillis)*(currentFrameMillis - lastFrameMillis))) < -100))  
  //acc = 1000*(((float)elbowHandDifferences.get(3) - (float)elbowHandDifferences.get(2)) / (((int)frameMillis.get(3) - (int)frameMillis.get(2))*((int)frameMillis.get(3) - (int)frameMillis.get(2))));
  //elbowHandDifferences.get(3) - elbowHandDifferences.get(2) 
println(  1000*((currentFrameElbowHandDifference - lastFrameElbowHandDifference) / ((currentFrameMillis - lastFrameMillis)*(currentFrameMillis - lastFrameMillis))));
  return 1000*((currentFrameElbowHandDifference - lastFrameElbowHandDifference) / ((currentFrameMillis - lastFrameMillis)*(currentFrameMillis - lastFrameMillis)));

}

// draw the skeleton with the selected joints
void drawSkeleton(int userId)
{
  strokeWeight(3);

  // to get the 3d joint data
  drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK);

  drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_LEFT_SHOULDER);
  drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_LEFT_ELBOW);
  drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, SimpleOpenNI.SKEL_LEFT_HAND);

  drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_RIGHT_SHOULDER);
  drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_RIGHT_ELBOW);
  drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, SimpleOpenNI.SKEL_RIGHT_HAND);

  drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_TORSO);
  drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_TORSO);

  drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_LEFT_HIP);
  drawLimb(userId, SimpleOpenNI.SKEL_LEFT_HIP, SimpleOpenNI.SKEL_LEFT_KNEE);
  drawLimb(userId, SimpleOpenNI.SKEL_LEFT_KNEE, SimpleOpenNI.SKEL_LEFT_FOOT);

  drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT_HIP);
  drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_HIP, SimpleOpenNI.SKEL_RIGHT_KNEE);
  drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_KNEE, SimpleOpenNI.SKEL_RIGHT_FOOT);  

  strokeWeight(1);
 
}

void drawLimb(int userId,int jointType1,int jointType2)
{
  PVector jointPos1 = new PVector();
  PVector jointPos2 = new PVector();
  float  confidence;
  
  // draw the joint position
  confidence = context.getJointPositionSkeleton(userId,jointType1,jointPos1);
  confidence = context.getJointPositionSkeleton(userId,jointType2,jointPos2);

  stroke(255,0,0,confidence * 200 + 55);
  line(jointPos1.x,jointPos1.y,jointPos1.z,
       jointPos2.x,jointPos2.y,jointPos2.z);
  
  drawJointOrientation(userId,jointType1,jointPos1,50);
}

void drawJointOrientation(int userId,int jointType,PVector pos,float length)
{
  // draw the joint orientation  
  PMatrix3D  orientation = new PMatrix3D();
  float confidence = context.getJointOrientationSkeleton(userId,jointType,orientation);
  if(confidence < 0.001f) 
    // nothing to draw, orientation data is useless
    return;
    
  pushMatrix();
    translate(pos.x,pos.y,pos.z);
    
    // set the local coordsys
    applyMatrix(orientation);
    
    // coordsys lines are 100mm long
    // x - r
    stroke(255,0,0,confidence * 200 + 55);
    line(0,0,0,
         length,0,0);
    // y - g
    stroke(0,255,0,confidence * 200 + 55);
    line(0,0,0,
         0,length,0);
    // z - b    
    stroke(0,0,255,confidence * 200 + 55);
    line(0,0,0,
         0,0,length);
  popMatrix();
}

// -----------------------------------------------------------------
// SimpleOpenNI user events

void onNewUser(int userId)
{
  println("onNewUser - userId: " + userId);
  println("  start pose detection");
  
  context.startPoseDetection("Psi",userId);
}

void onLostUser(int userId)
{
  println("onLostUser - userId: " + userId);
}

void onStartCalibration(int userId)
{
  println("onStartCalibration - userId: " + userId);
}

void onEndCalibration(int userId, boolean successfull)
{
  println("onEndCalibration - userId: " + userId + ", successfull: " + successfull);
  
  if (successfull) 
  { 
    println("  User calibrated !!!");
    context.startTrackingSkeleton(userId); 
  } 
  else 
  { 
    println("  Failed to calibrate user !!!");
    println("  Start pose detection");
    context.startPoseDetection("Psi",userId);
  }
}

void onStartPose(String pose,int userId)
{
  println("onStartdPose - userId: " + userId + ", pose: " + pose);
  println(" stop pose detection");
  
  context.stopPoseDetection(userId); 
  context.requestCalibrationSkeleton(userId, true);
 
}

void onEndPose(String pose,int userId)
{
  println("onEndPose - userId: " + userId + ", pose: " + pose);
}

// -----------------------------------------------------------------
// Keyboard events

void keyPressed()
{
  switch(key)
  {
  case ' ':
    context.setMirror(!context.mirror());
    break;
  }
    
  switch(keyCode)
  {
    case LEFT:
      rotY += 0.1f;
      break;
    case RIGHT:
      // zoom out
      rotY -= 0.1f;
      break;
    case UP:
      if(keyEvent.isShiftDown())
        zoomF += 0.01f;
      else
        rotX += 0.1f;
      break;
    case DOWN:
      if(keyEvent.isShiftDown())
      {
        zoomF -= 0.01f;
        if(zoomF < 0.01)
          zoomF = 0.01;
      }
      else
        rotX -= 0.1f;
      break;
  }
}

